#pragma config(Sensor, in1,    ClawMeasurer,   sensorPotentiometer)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl8,  spyplaneSensor, sensorSONAR_mm)
#pragma config(Motor,  port1,           rightMotor,    tmotorVex393, openLoop)
#pragma config(Motor,  port6,           clawMotor,     tmotorServoStandard, openLoop)
#pragma config(Motor,  port10,          leftMotor,     tmotorVex393, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*----------------------------------------------------------------------------------------------------*\
|*                                 - Moving Straight with Encoders -                                  *|
|*                                      ROBOTC on VEX 2.0 CORTEX                                      *|
|*                                                                                                    *|
|*  This program implements a self-straightening algorithm that provides a higher power level to the  *|
|*  motor that has traveled less, determined by comparing the values of the two encoders.             *|
|*  There is a 2 second pause at the beginning of the program.                                        *|
|*                                                                                                    *|
|*                                        ROBOT CONFIGURATION                                         *|
|*    NOTES:                                                                                          *|
|*    1)  Reversing 'rightMotor' (port 2) in the "Motors and Sensors Setup" is needed with the        *|
|*        "Squarebot" model, but may not be needed for all robot configurations.                      *|
|*    2)  Whichever encoder is being used for feedback should be cleared just before it starts        *|
|*        counting by using the "SensorValue(encoder) = 0;".  This helps ensure consistancy.          *|
|*                                                                                                    *|
|*    MOTORS & SENSORS:                                                                               *|
|*    [I/O Port]          [Name]              [Type]                [Description]                     *|
|*    Motor   - Port 2    rightMotor          VEX 3-wire module     Right side motor                  *|
|*    Motor   - Port 3    leftMotor           VEX 3-wire module     Left side motor                   *|
|*    Digital - Port 1,2  rightEncoder        VEX Shaft Encoder     Right side                        *|
|*    Digital - Port 3,4  leftEncoder         VEX Shaft Encoder     Left side                         *|
\*----------------------------------------------------------------------------------------------------*/


//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++
task main()
{
	wait1Msec(2000);							// Robot waits for 2 seconds before executing program
	while(SensorValue[spyplaneSensor] > 150)
		//this is the value the ultrasonic sensor would have so the claw wouldn't close.
	{
		motor[clawMotor] = 127;
	}

	SensorValue[rightEncoder] = 0;	  // Set the right encoder so that it starts counting at 0
	SensorValue[leftEncoder]  = 0;	  // Set the left encoder so that it starts counting at 0

	while(SensorValue[rightEncoder] <= 4254 || SensorValue[leftEncoder] <= 4254)		//these are the values needed to traverse the first tape line
	{
		if(SensorValue[rightEncoder] == SensorValue[leftEncoder]) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[rightMotor] = 95;		    // Right Motor is run at power level over 9000
			motor[leftMotor]  = 95;		    // Left Motor is run at power level 80
		}
		else if(SensorValue[rightEncoder] > SensorValue[leftEncoder])	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[rightMotor] = 75;		    // Right Motor is run at power level 60
			motor[leftMotor]  = 95;		    // Left Motor is run at power level 80
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[rightMotor] = 95;		    // Right Motor is run at power level 80
			motor[leftMotor]  = 75;		    // Left Motor is run at power level 60
		}
	}
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	//stops the motor

	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;
	//resets the optical shaft encoders

	wait1Msec(1000);
	//robot waits for 1 second for it to completely stop
	while(SensorValue[rightEncoder] > -417 || SensorValue[leftEncoder] < 417)
		//these are the values of the encoders to make the robot turn 90 degrees
	{
		motor[rightMotor] = -80;
		motor[leftMotor] = 80;
		//this is the standard speed that is going to be used
		if(SensorValue[rightEncoder] <= -417)
		//if the right motor (attached with the right encoder) goes below the value needed to turn 90 degrees, the motor will stop
		{
			motor[rightMotor] = 0;
		}
		if(SensorValue[leftEncoder] >= 417)
		//if the left motor (attached with the left encoder) goes above the value needed to turn 90 degrees, the motor will stop
		{
			motor[leftMotor] = 0;
			//stil neds moar pones
		}
	}
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	//stops the motors
	wait1Msec(1000);
	//waits 1 second before the next step
	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;
	//resets the encoder values

	while(SensorValue[rightEncoder] <= 1835 || SensorValue[leftEncoder] <= 1835)
		//these are the values needed to make the robot get to the end of the second line of tape
	{
		if(SensorValue[rightEncoder] == SensorValue[leftEncoder])
			//if the sensor values of the right encoder equals the left encoder, the motors will both move at the same speed
		{
			motor[rightMotor] = 95;
			motor[leftMotor] = 95;
			//standard speeds for the motors
		}
		else if(SensorValue[rightEncoder] < SensorValue[leftEncoder])
			//if the right motor is going faster than the left motor, the right motor will slow down so the robot will go straight
		{
			motor[rightMotor] = 75;
			motor[leftMotor] = 95;
		}
		else
			//if the left motor is going faster than the right motor, the left motor will slow down so the robot will stay straight
		{
			motor[rightMotor] = 95;
			motor[leftMotor] = 75;
		}
	}
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	//stops both of the motors
	SensorValue[leftEncoder] = 0;
	SensorValue[rightEncoder] = 0;
	//this resets both of the encoders

	wait1Msec(1000);
	//the robot stays still for 1 second
	SensorValue[ClawMeasurer] = 0;
	//this sets the claw's potentiometer to 0
	while(SensorValue[spyplaneSensor] <=150)
	{
		motor[clawMotor] = -127;
	}
//so, when the patient woke up, his skeleton was missing and the doctor was never heard from again! well, anyways, that's how i lost my medical liscence. Archimedes, NO! it's filthy in there! Hah, birds. Now most hearts wouldn't be able to withstand this voltage, but i'm quite certain your heart will *splat!*. "what was noise?" the sound of progress my friend. So, let's see. "kill me" later. Hahhahahahahaha. "ha..ha..ha?" ooh, that looks good. *plop* very nice there. "Should i be awake for this?" well, no. but as long as you are, could you hold your ribcage open a bit, i can't seem to *splat, squish* "Ahhh" oh, don't be such a baby, ribs grow back. no they don't.
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
